// Rotate3D.h

#pragma once

#ifndef _ROTATE_3D_HH_
#define _ROTATE_3D_HH_


#include "Matrix3d.h"
#include "inc.h"

namespace Rotate3dN
{
//	using namespace RavlN;

	const RealT axis_threshold = 1E-12;

	// Exponential map (axis-angle)
	void ExpToMatrix(Matrix3dC& R, RealT x, RealT y, RealT z);
	void ExpToMatrix(Matrix3dC& R, const Vector3dC& axis);
	void MatrixToExp(Vector3dC& a, const Matrix3dC& R);
	void ExpToMatrix_J(Matrix3dC& dR, const Vector3dC& axis, UIntT dParam);

	// Quarternions
	void QuartToMatrix(Matrix3dC& R, const Vector4dC& q);

	// Euler angles

	void MatrixToEulerXYZ(Vector3dC& a, const Matrix3dC& R, RealT a2_default = 0.0);
	void MatrixToEulerZXY(Vector3dC& a, const Matrix3dC& R, RealT a1_default = 0.0);
	void MatrixToEulerYXZ(Vector3dC& a, const Matrix3dC& R, RealT a2_default = 0.0);

	void EulerXYZToMatrix(Matrix3dC& R, RealT a0, RealT a1, RealT a2);
	void EulerYXZToMatrix(Matrix3dC& R, RealT a0, RealT a1, RealT a2);

	void EulerXYZToMatrix_da0(Matrix3dC& R, RealT a0, RealT a1, RealT a2);
	void EulerXYZToMatrix_da1(Matrix3dC& R, RealT a0, RealT a1, RealT a2);
	void EulerXYZToMatrix_da2(Matrix3dC& R, RealT a0, RealT a1, RealT a2);

	// inline function expansions

	inline void ExpToMatrix(Matrix3dC& R, const Vector3dC& axis)
	{ ExpToMatrix(R, axis[0], axis[1], axis[2]); } 

	inline void EulerXYZToMatrix(Matrix3dC R, const Vector3dC& a)
	{ EulerXYZToMatrix(R, a[0], a[1], a[2]); }

	inline void EulerXYZToMatrix_da0(Matrix3dC& R, const Vector3dC& a)
	{ EulerXYZToMatrix_da0(R, a[0], a[1], a[2]);}

	inline void EulerXYZToMatrix_da1(Matrix3dC& R, const Vector3dC& a)
	{ EulerXYZToMatrix_da1(R, a[0], a[1], a[2]);}

	inline void EulerXYZToMatrix_da2(Matrix3dC& R, const Vector3dC& a)
	{ EulerXYZToMatrix_da2(R, a[0], a[1], a[2]); }

	inline void EulerYXZToMatrix(Matrix3dC& R, const Vector3dC& a)
	{ EulerYXZToMatrix(R, a[0], a[1], a[2]); }
}

#endif
